#include <limits> // 包含 INT_MIN 和 INT_MAX


#define LEFT_MOTOR_DIR_A 2
#define LEFT_MOTOR_DIR_B 3
#define LEFT_MOTOR_SPEED 9
#define RIGHT_MOTOR_DIR_A 4
#define RIGHT_MOTOR_DIR_B 5
#define RIGHT_MOTOR_SPEED 10


#define LEFT_SENSOR A0
#define RIGHT_SENSOR A1

// 定义读取次数
#define READINGS_COUNT 10

// 计算修剪后的平均值
float calculateTrimmedMean(int temp_val[], int times) {
  int max = std::numeric_limits<int>::min(); // 初始化最大值为整型最小值
  int min = std::numeric_limits<int>::max(); // 初始化最小值为整型最大值
  float sum = 0.0; // 初始化求和变量为0

  // 遍历数组，找出最大值和最小值，并计算总和
  for (int t = 0; t < times; t++) {
    if (temp_val[t] > max) {
      max = temp_val[t]; // 更新最大值
    }
    if (temp_val[t] < min) {
      min = temp_val[t]; // 更新最小值
    }
    sum += temp_val[t]; // 累加当前值
  }

  // 计算去除最大值和最小值后的平均值
  float lvbozhi = (sum - max - min) / static_cast<float>(times - 2);

  return lvbozhi; // 返回修剪后的平均值
}

void setup() {
  // 初始化串口通信
  Serial.begin(9600);

  // 初始化电机驱动器引脚
  pinMode(LEFT_MOTOR_DIR_A, OUTPUT);
  pinMode(LEFT_MOTOR_DIR_B, OUTPUT);
  pinMode(LEFT_MOTOR_SPEED, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR_A, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR_B, OUTPUT);
  pinMode(RIGHT_MOTOR_SPEED, OUTPUT);

  // 初始化模拟传感器引脚
  pinMode(LEFT_SENSOR, INPUT);
  pinMode(RIGHT_SENSOR, INPUT);
}

void loop() {
  // 读取左侧模拟传感器的值，取平均值
  int leftSensorValues[READINGS_COUNT];
  for (int t = 0; t < READINGS_COUNT; t++) {
    leftSensorValues[t] = analogRead(LEFT_SENSOR); // 读取左侧传感器值
    delayMicroseconds(10); // 等待一段时间，避免读取过快导致误差
  }

  // 读取右侧模拟传感器的值，取平均值
  int rightSensorValues[READINGS_COUNT];
  for (int t = 0; t < READINGS_COUNT; t++) {
    rightSensorValues[t] = analogRead(RIGHT_SENSOR); // 读取右侧传感器值
    delayMicroseconds(10); // 等待一段时间，避免读取过快导致误差
  }

  // 计算修剪后的平均值
  float leftTrimmedMean = calculateTrimmedMean(leftSensorValues, READINGS_COUNT);
  float rightTrimmedMean = calculateTrimmedMean(rightSensorValues, READINGS_COUNT);

  // 根据传感器的差值调整电机速度
  adjustSpeed(leftTrimmedMean, rightTrimmedMean);

  // 显示传感器值
  Serial.print("Left Trimmed Mean: ");
  Serial.print(leftTrimmedMean);
  Serial.print(", Right Trimmed Mean: ");
  Serial.println(rightTrimmedMean);
}

void adjustSpeed(float leftTrimmedMean, float rightTrimmedMean) {
  int leftSpeed = 128; // 中心速度
  int rightSpeed = 128; // 中心速度

  // 调整电机速度
  int diff = rightTrimmedMean - leftTrimmedMean;

  // 如果左侧传感器值较高，向右转
  if (diff > 0) {
    leftSpeed += abs(diff / 2);
    rightSpeed -= abs(diff / 2);
  }
  // 如果右侧传感器值较高，向左转
  else if (diff < 0) {
    leftSpeed -= abs(diff / 2);
    rightSpeed += abs(diff / 2);
  }

  // 设置电机方向
  digitalWrite(LEFT_MOTOR_DIR_A, HIGH);
  digitalWrite(LEFT_MOTOR_DIR_B, LOW);
  digitalWrite(RIGHT_MOTOR_DIR_A, HIGH);
  digitalWrite(RIGHT_MOTOR_DIR_B, LOW);

  // 设置电机速度
  analogWrite(LEFT_MOTOR_SPEED, constrain(leftSpeed, 0, 255));
  analogWrite(RIGHT_MOTOR_SPEED, constrain(rightSpeed, 0, 255));
}